#include "motor_ctrl.h"
/**
 * AK电机: 
 *      定义顺时针为电机(位置、速度、扭矩)的正值增长方向
 * 参数：
 *      顺时针 为 位置 增大方向，
 *      逆时针 为 速度 增大方向，(顺时针旋转时速度参数为负值，逆时针旋转时速度参数为正值)
 *      逆时针 为 扭矩 增大方向，(顺时针旋转时扭矩参数为负值，逆时针旋转时扭矩参数为正值)
 * 注意：
 *      位置速度环(混合)模式时，给的速度值必须为正值，反馈的速度值则符合参数中的表述
 *      位置速度环和速度环给的速度不一样，前者要再除以10
 *      电机反馈的扭矩为绝对值, 且要转子卡住情况下，反馈的值与设定的值相等
 *      运动学正方向为逆时针
 *      位置、速度、力矩模式超时1s
 *      重新上电后，不会记忆圈数，即 %360
 * */ 

Motor_AK::Motor_AK(Motor_ID_Info &info) : Motor(info)
{   
    std::cout<<"Motor_AK"<<std::endl;
    this->accept_err        = 0.2f;
    this->each_angle_max    = 5.0f;
    this->cmd_freq          = AK_FREQ;
    this->dir               = DIR_CLOCKWISE;
}

Motor_AK::~Motor_AK()
{
    std::cout<<"~Motor_AK"<<std::endl;
}

int32_t Motor_AK:: set_origin(uint8_t mode)
{
    return 0;
}
                                       
int32_t Motor_AK:: set_motor_pos(double pos)
{
#define POS_MIN       (-36000)
#define POS_MAX       (36000)
    if((pos >= POS_MIN) && (pos <= POS_MAX)) {
        this->pos = pos;
        return (int32_t)(pos * 10000);
    } else {
        this->pos = 0;
        return 0;
    }
}

int32_t Motor_AK:: set_motor_w(double w)
{
    /* 电气转速(erpm)--->转速(rpm)--->角速度(w)
        erpm/减速比/极对数 = rpm
        rpm/60*360 = w (°/s)
        rpm/60*6.28f = w (rad/s)
    */
#define W_MIN       (-100000.0f/AK_60_6_V_RATIO/AK_60_6_POLES*6.0f)
#define W_MAX       (100000.0f/AK_60_6_V_RATIO/AK_60_6_POLES*6.0f)
#define W_MIX_MIN   (-32768.0f/AK_60_6_V_RATIO/AK_60_6_POLES*6.0f)
#define W_MIX_MAX   (32768.0f/AK_60_6_V_RATIO/AK_60_6_POLES*6.0f)

    if(flag_mix == 0) {             //速度模式
        if((w >= W_MIN) && (w <= W_MAX)) {
            this->w = w;
            return (int32_t)(-w/6.0f*AK_60_6_V_RATIO*AK_60_6_POLES);      //单速度模式,ak电机顺时针旋转时是负速度值;
        } else {
            this->w = 0;
            return 0;
        }
    } else {
        if((w >= W_MIX_MIN) && (w <= W_MIX_MAX)) {  //位置速度模式
            this->w = w;
            return (int32_t)(w/6.0f*AK_60_6_V_RATIO*AK_60_6_POLES/10.0f);   //位置速度模式时，另外还要放缩10倍
        } else { 
            this->w = 0;
            return 0;
        }    
    }
} 
int32_t Motor_AK:: set_motor_t(double t)
{
#define IA_MIN       (-60)
#define IA_MAX       (60)
    double ia = (t/AK_60_6_T_CONST/AK_60_6_V_RATIO);
    if((ia >= IA_MIN) && (ia <= IA_MAX)) {
        this->t = t;
        return (int32_t)(-ia * 1000);
    } else {
        this->t = 0;
        return 0;
    }
} 
int32_t Motor_AK:: set_motor_acc(double acc)
{
#define ACC_MIN       (0)
#define ACC_MAX       (327670.0f/AK_60_6_V_RATIO/AK_60_6_POLES*6.0f)
    if((acc >= ACC_MIN) && (acc <= ACC_MAX)) {
        this->acc = acc;
        return (int32_t)(acc/6.0f*AK_60_6_V_RATIO*AK_60_6_POLES/10.0f);
    } else {
        this->acc = acc;
        return 0;
    }
}

int32_t Motor_AK:: set_motor_kp(double kp)
{
    return 0;
}  
int32_t Motor_AK:: set_motor_kd(double kd)
{
    return 0;
}

void Motor_AK::set_cur_tau(uint8_t data[])
{
    short cur_int = ((data[0] <<8)| data[1]);
    this->cur_tau =  cur_int/100.0f*AK_60_6_T_CONST*AK_60_6_V_RATIO;

}

void Motor_AK::set_cur_pos(uint8_t data[])
{
    int16_t pos = (data[0] << 8) | data[1];
    this->cur_pos = (pos*0.1f);
}

void Motor_AK::set_cur_w(uint8_t data[])
{
    int16_t w = (data[0] << 8) | data[1];
    this->cur_w = -(w*10.0f/AK_60_6_V_RATIO/AK_60_6_POLES*6.0f);   
}

int32_t Motor_AK::set_motor_dataframe(uint8_t dataframe[], Motor_Mode mode, double value[], uint8_t len)
{
    int32_t data_len = 0;
    int32_t val;
    switch (mode) {
        case MOTOR_MODE_ENTER:
        case MOTOR_MODE_EXIT:
            data_len = 0;
            break;
        case MOTOR_MODE_ORIGIN:
            data_len = 1;
            dataframe[0] = 0;         //0-临时 1-永久 2-恢复默认
            break;
        case MOTOR_MODE_W:  //value为角速度
            data_len = 4;
            if(1 > len) {
                cout << "MOTOR_TYPE_AK MOTOR_MODE_W len error!" << endl;
                return -1;
            }
            val = set_motor_w(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            break; 
        case MOTOR_MODE_POS_HOLD:    //value为位置
        case MOTOR_MODE_POS_TRACK:
            data_len = 4;
            if(1 > len) {
                cout << "MOTOR_TYPE_AK MOTOR_MODE_POS len error!" << endl;
                return -1;
            }
            val = set_motor_pos(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            break;
        case MOTOR_MODE_T:  //value为扭矩
            data_len = 4;
            if(1 > len) {
                cout << "MOTOR_TYPE_AK MOTOR_MODE_T len error!" << endl;
                return -1;
            }
            val = set_motor_t(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            break;   
        case MOTOR_MODE_MIX:    /* value 依次是 位置 速度 加速度 */
            data_len = 8;
            flag_mix = 1;
            if(3 > len) {
                cout << "MOTOR_TYPE_AK MOTOR_MODE_MIX len error!" << endl;
                return -1;
            }
            val = set_motor_pos(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;

            val = set_motor_w(value[1]);
            dataframe[4] =  (val>>8) & 0xff;
            dataframe[5] =  val & 0xff;

            val = set_motor_acc(value[2]);
            dataframe[6] =  (val>>8) & 0xff;
            dataframe[7] =  val & 0xff;
            flag_mix = 0;
            break; 
        default:
            break;
    }
#if 0
    printf("dataframe=");
    for(uint8_t i =0;i<8;i++) {
        printf("0x%02x ",dataframe[i]);
    }
    printf("\n");
#endif
    return data_len;
}


uint8_t Motor_AK::get_motor_mode_map(Motor_Mode mode)
{
    uint8_t ret=0;
    switch(mode) {
        case MOTOR_MODE_ENTER:
        case MOTOR_MODE_EXIT:
            ret = 0;
            break;
        case MOTOR_MODE_ORIGIN:
            ret = 5;
            break; 
        case MOTOR_MODE_W:
            ret = 3;
            break;
        case MOTOR_MODE_POS_HOLD:
        case MOTOR_MODE_POS_TRACK:
            ret = 4;
            break;
        case MOTOR_MODE_T:
            ret = 1;
            break;
        case MOTOR_MODE_MIX:
            ret = 6;
            break;
        default:
            break;
    }
    return ret;
}